/*
 * StartApplication.java
 *
 * Created on October 8, 2006, 12:15 AM
 */
package org.sunspotworld;

import com.sun.spot.io.j2me.radiogram.Radiogram;
import com.sun.spot.io.j2me.radiogram.RadiogramConnection;
import com.sun.spot.peripheral.IDriver;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.radio.RadioFactory;
import com.sun.spot.peripheral.radio.RadioPolicyManager;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ILightSensor;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.sensorboard.peripheral.Servo;
import com.sun.spot.util.BootloaderListener;
import com.sun.spot.util.Utils;
import java.io.IOException;
import javax.microedition.io.Connector;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * A Robot Lazor Game
 * @author Douglas McClusky, Tomas Henriquez, Sabin Larrañaga
 *
 */
public class StartApplication extends MIDlet implements IDriver {
    public static final int CONNECT_PORT = 23;
    public static final int RUN_PORT = 20;

    public static final byte ROBOT_SIGNATURE = 4;
    public static final byte LAZER_SIGNATURE = 32;
    public static final byte CONTROL_SIGNATURE = 17;

    private static final short PID = 4;
    private static final int CHANNEL = 23;

    private static final int DEATH_LIGHT_VALUE = 15;

    private final int HALF_RANGE = 500;
    private Servo[] wheels = {new Servo(EDemoBoard.getInstance().getOutputPins()[0]), new Servo(EDemoBoard.getInstance().getOutputPins()[1])};
    private int[] stop = new int[2];
    private ITriColorLED[] leds = EDemoBoard.getInstance().getLEDs();
    private boolean servoActive = false;
    private Radiogram rrg;
    private boolean flag = false;
    private ILightSensor sensorLigth = EDemoBoard.getInstance().getLightSensor();
    private RadiogramConnection conn = null;

    private int wheel1 = 1451,wheel2 = 1454;

    private String controlAddr;

    private void connect() {
        try {
            conn = (RadiogramConnection) Connector.open("radiogram://:"+CONNECT_PORT);
            rrg = (Radiogram)conn.newDatagram(conn.getMaximumLength());
            Radiogram xrg = (Radiogram)conn.newDatagram(conn.getMaximumLength());
            xrg.reset();
            xrg.writeByte(ROBOT_SIGNATURE);
            conn.receive(rrg);
            if (rrg.readByte() != CONTROL_SIGNATURE) {
                return;
            }
            xrg.setAddress(rrg);
            conn.send(xrg);
            controlAddr = rrg.getAddress();
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        if (conn != null) {
            try {
                conn.close();
                conn = null;
                rrg = null;
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }

    private void blinkConnected() {
        for (int i=0; i<3; i++) {
            for (int j=0; j<leds.length; j++) {
                leds[j].setColor(LEDColor.GREEN);
                leds[j].setOn();
            }
            Utils.sleep(500);
            for (int j=0; j<leds.length; j++) {
                leds[j].setOff();
            }
            Utils.sleep(500);
        }
    }

    /**
     * Midlet execution starts here
     * @throws javax.microedition.midlet.MIDletStateChangeException
     */
    protected void startApp() throws MIDletStateChangeException {
        new BootloaderListener().start();   // monitor the USB (if connected) and recognize commands from host

        RadioPolicyManager rpm = (RadioPolicyManager) RadioFactory.getRadioPolicyManager();
        rpm.setPanId(PID);
        rpm.setChannelNumber(CHANNEL);

        leds[0].setColor(LEDColor.RED);
        leds[0].setOn();
        
        while (controlAddr == null) {
            connect();
        }
        blinkConnected();
        
        Spot.getInstance().getDriverRegistry().add(this);  //have the SPOT check with us before going to deep sleep
        stop[0] = wheel1;  //the pulse width that holds servo 0 still (actual values will vary by servo)
        stop[1] = wheel2;  //the pulse width that holds servo 1 still (actual values will vary by servo)

        for (int i = 0; i < 2; i++) {
            wheels[i].setBounds(stop[i] - HALF_RANGE, stop[i] + HALF_RANGE);
        }

        int lightz = 0;

        while (true) {
            try {

                conn = (RadiogramConnection) Connector.open("radiogram://:"+RUN_PORT);

                rrg = (Radiogram) conn.newDatagram(conn.getMaximumLength());
                double value = 0;
                while (true) {
                    rrg.reset();
                    value = 0;
                    conn.receive(rrg);
                    lightz = sensorLigth.getAverageValue();

                    if (lightz > DEATH_LIGHT_VALUE) {
                        die();//the robot enters in a state of comma for 5 seconds.
                    }


                    value = rrg.readDouble();


                    if (value == Double.MIN_VALUE) {
                        reverse();
                    } else if (value == Double.MAX_VALUE) {
                        notifyDestroyed();
                        break;
                    } else if (value >= 5) {
                        turnRigth(value - 5);
                    } else if (value < -5) {
                        turnLeft(value + 5);
                    } else if (value < 1 && value > -1 && value != 0.0) {
                        rotate(value);
                    } else if (value == 0.0 && flag == false) {
                        flag = true;
                        stop();
                    }



                }
            } catch (Exception ex) {
                ex.printStackTrace();
            } finally {
                try {
                    if (conn != null) {
                        conn.close();
                        conn = null;
                    }
                } catch (IOException ex) {
                    ex.printStackTrace();
                }
            }

        }
    }

    private void ledsOn(LEDColor color, int length) {
        for (int i = 0; i < length; i++) {
            leds[i].setColor(color);
            leds[i].setOn();
        }
    }

    private void ledsOff() {
        for (int i = 0; i < leds.length; i++) {
            leds[i].setOff();
        }
    }

    private void die() {
        sendDeath();
        ledsOn(new LEDColor(30, 0, 0), 8);

        /*        for (int i = 0; i <= 7; i++) {
        leds[i].setRGB(30, 0, 0);
        leds[i].setOn();
        }*/
        stop();
        Utils.sleep(5000);

        ledsOff();

    }

    /**
     * hold the servos still for a given amount of time
     * @param ms - number of milliseconds to brake
     */
    private void brake() {
        servoActive = true;
        for (int i = 0; i < 2; i++) {
            wheels[i].setPosition((float) .5);  //position .5 corresponds to the stop value above
        }
        Utils.sleep(50);
        servoActive = false;

    }

    /* Here we use  the values thrown by the accelerometer and transform them
     * so we can use those values in the speed of the wheels
     */
    private double conversionRotate(double value) {
        return (value / 2) + 0.5f;

    }

    private double conversionTurn(double value) {
        return (value / 2);

    }

    private void rotate(double value) {

        flag = false;
        double power = conversionRotate(value);
        servoActive = true;
        wheels[0].setPosition((float) power);
        wheels[1].setPosition((float) power);
        servoActive = false;

    }

    /**
     * The motor Spot sends a death sign to the remote control so he can rest 1 life.
     *
     */
    private void sendDeath() {

        RadiogramConnection conn = null;
        try {
            conn = (RadiogramConnection) Connector.open("radiogram://" + controlAddr + ":"+RUN_PORT);
            Radiogram dg = (Radiogram)conn.newDatagram(conn.getMaximumLength());
            dg.reset();
            dg.writeBoolean(true);
            conn.send(dg);

        } catch (IOException ex) {
            ex.printStackTrace();
        } finally {
            try {
                if (conn != null) {
                    conn.close();
                    conn = null;
                }
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }

    private void turnLeft(double value) {

        flag = false;
        double power = conversionTurn(value);
        servoActive = true;
        wheels[0].setPosition((float) (-power));
        wheels[1].setPosition(1);
        servoActive = false;
    }

    private void turnRigth(double value) {

        flag = false;
        double power = conversionTurn(value);
        servoActive = true;
        wheels[0].setPosition(0);
        wheels[1].setPosition((float) (1 - power));
        servoActive = false;

    }

    private void reverse() {

        flag = false;
        servoActive = true;
        wheels[0].setPosition(1);
        wheels[1].setPosition(0);
        servoActive = false;

    }

    /**
     * Brake for a quarter second and then turn off the servos
     */
    private void stop() {
        brake();

        for (int i = 0; i < 2; i++) {
            wheels[i].setValue(0);  //set servo pins low
        }
    }

    //
    //IDriver methods
    //
    /**
     * Implements getDriverName of IDriver interface
     * @return the name of this Driver
     */
    public String getDriverName() {
        return "ServoDemo";
    }

    /**
     * Check if robot is ready for SPOT to deep sleep
     * @return whether the servos need to sustain a pulse
     */
    public boolean tearDown() {
        return !servoActive;
    }

    /**
     * what to do when SPOT comes out of deep sleep
     */
    public void setUp() {
        //do nothing
    }

    /**
     * what to do when SPOT shuts down
     */
    public void shutDown() {
        //do nothing
    }

    //
    //MIDLet methods
    //
    protected void pauseApp() {
        // This will never be called by the Squawk VM
    }

    protected void destroyApp(boolean arg0) throws MIDletStateChangeException {
        stop();
    }
}
